#include "Stepper.h"

// 28BYJ48 步进电机双四拍驱动表（每次激活两相，扭矩更大）
// 电机A的引脚映射表（PB5~PB8）
const uint16_t MotorA_StepTable[4] = {
    MOTOR_A_PIN1 | MOTOR_A_PIN2,                  // 0b0011 - AB相
    MOTOR_A_PIN2 | MOTOR_A_PIN3,                  // 0b0110 - BC相  
    MOTOR_A_PIN3 | MOTOR_A_PIN4,                  // 0b1100 - CD相
    MOTOR_A_PIN4 | MOTOR_A_PIN1                   // 0b1001 - DA相
};

// 电机B的引脚映射表（PA0~PA3）
const uint16_t MotorB_StepTable[4] = {
    MOTOR_B_PIN1 | MOTOR_B_PIN2,                  // 0b0011 - AB相
    MOTOR_B_PIN2 | MOTOR_B_PIN3,                  // 0b0110 - BC相  
    MOTOR_B_PIN3 | MOTOR_B_PIN4,                  // 0b1100 - CD相
    MOTOR_B_PIN4 | MOTOR_B_PIN1                   // 0b1001 - DA相
};

// 电机A控制变量
static StepperState_t motor_a_state = STEPPER_STOP;
static uint32_t motor_a_speed = 2;  // 默认速度，单位ms
static int motor_a_current_step = 0;

// 电机B控制变量
static StepperState_t motor_b_state = STEPPER_STOP;
static uint32_t motor_b_speed = 2;  // 默认速度，单位ms
static int motor_b_current_step = 0;

// 获取指定电机的GPIO和引脚
static void GetMotorPins(MotorSelect_t motor, GPIO_TypeDef** gpio, uint16_t* pins)
{
    if (motor == MOTOR_A) {
        *gpio = MOTOR_A_GPIO;
        *pins = MOTOR_A_PIN1 | MOTOR_A_PIN2 | MOTOR_A_PIN3 | MOTOR_A_PIN4;
    } else {
        *gpio = MOTOR_B_GPIO;
        *pins = MOTOR_B_PIN1 | MOTOR_B_PIN2 | MOTOR_B_PIN3 | MOTOR_B_PIN4;
    }
}

// 获取指定电机的步进表
static const uint16_t* GetMotorStepTable(MotorSelect_t motor)
{
    return (motor == MOTOR_A) ? MotorA_StepTable : MotorB_StepTable;
}

// 获取指定电机的状态变量指针
static StepperState_t* GetMotorState(MotorSelect_t motor)
{
    return (motor == MOTOR_A) ? &motor_a_state : &motor_b_state;
}

// 获取指定电机的速度变量指针
static uint32_t* GetMotorSpeed(MotorSelect_t motor)
{
    return (motor == MOTOR_A) ? &motor_a_speed : &motor_b_speed;
}

// 获取指定电机的当前步数变量指针
static int* GetMotorCurrentStep(MotorSelect_t motor)
{
    return (motor == MOTOR_A) ? &motor_a_current_step : &motor_b_current_step;
}

// 双电机GPIO初始化
void Stepper_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    
    // 使能GPIOA和GPIOB时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
    
    // 配置电机A引脚（PB5~PB8）
    GPIO_InitStructure.GPIO_Pin = MOTOR_A_PIN1 | MOTOR_A_PIN2 | MOTOR_A_PIN3 | MOTOR_A_PIN4;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MOTOR_A_GPIO, &GPIO_InitStructure);
    GPIO_ResetBits(MOTOR_A_GPIO, MOTOR_A_PIN1 | MOTOR_A_PIN2 | MOTOR_A_PIN3 | MOTOR_A_PIN4);
    
    // 配置电机B引脚（PA0~PA3）
    GPIO_InitStructure.GPIO_Pin = MOTOR_B_PIN1 | MOTOR_B_PIN2 | MOTOR_B_PIN3 | MOTOR_B_PIN4;
    GPIO_Init(MOTOR_B_GPIO, &GPIO_InitStructure);
    GPIO_ResetBits(MOTOR_B_GPIO, MOTOR_B_PIN1 | MOTOR_B_PIN2 | MOTOR_B_PIN3 | MOTOR_B_PIN4);
    
    // 初始化状态变量
    motor_a_state = STEPPER_STOP;
    motor_b_state = STEPPER_STOP;
    motor_a_current_step = 0;
    motor_b_current_step = 0;
}

// 步进电机单步驱动函数
void Stepper_Step(MotorSelect_t motor, int step)
{
    GPIO_TypeDef* gpio;
    uint16_t pins;
    const uint16_t* step_table = GetMotorStepTable(motor);
    
    GetMotorPins(motor, &gpio, &pins);
    
    // 先清零所有输出
    GPIO_ResetBits(gpio, pins);
    // 输出当前步的相应引脚
    GPIO_SetBits(gpio, step_table[step & 0x03]);
}

// 设置步进电机状态
void Stepper_SetState(MotorSelect_t motor, StepperState_t state, uint32_t speed)
{
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    
    *motor_state = state;
    *motor_speed = speed;
}

// 停止步进电机
void Stepper_Stop(MotorSelect_t motor)
{
    StepperState_t* motor_state = GetMotorState(motor);
    GPIO_TypeDef* gpio;
    uint16_t pins;
    
    *motor_state = STEPPER_STOP;
    GetMotorPins(motor, &gpio, &pins);
    // 清零所有输出，停止电机
    GPIO_ResetBits(gpio, pins);
}

// 步进电机处理函数 - 使用精确延时的阻塞方式
void Stepper_Process(MotorSelect_t motor)
{
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    int* current_step = GetMotorCurrentStep(motor);
    
    if (*motor_state == STEPPER_STOP) {
        return;
    }
    
    // 执行一步
    if (*motor_state == STEPPER_FORWARD) {
        *current_step = (*current_step + 1) & 0x03;
    } else if (*motor_state == STEPPER_BACKWARD) {
        *current_step = (*current_step + 3) & 0x03;
    }
    
    Stepper_Step(motor, *current_step);
    
    // 使用Delay.c中的精确延时函数
    Delay_ms(*motor_speed);
}

// 步进电机持续正转函数（保留原有接口）
void Stepper_Forward(MotorSelect_t motor, uint32_t speed)
{
    int step = 0;
    while(1) {
        step = (step + 1) & 0x03;
        Stepper_Step(motor, step);
        Delay_ms(speed);
    }
}

// 步进电机持续反转函数（保留原有接口）
void Stepper_Backward(MotorSelect_t motor, uint32_t speed)
{
    int step = 0;
    while(1) {
        step = (step + 3) & 0x03;
        Stepper_Step(motor, step);
        Delay_ms(speed);
    }
}

// 步进电机正转360度函数
void Stepper_Forward360(MotorSelect_t motor, uint32_t speed)
{
    // 保存当前状态
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    StepperState_t previous_state = *motor_state;
    uint32_t previous_speed = *motor_speed;
    
    // 设置正转状态
    Stepper_SetState(motor, STEPPER_FORWARD, speed);
    
    // 执行2048步完成360度旋转
    for(int i = 0; i < 2048; i++) {
        Stepper_Process(motor);
    }
    
    // 停止电机
    Stepper_Stop(motor);
    
    // 恢复之前的状态
    *motor_state = previous_state;
    *motor_speed = previous_speed;
}

// 步进电机反转360度函数
void Stepper_Backward360(MotorSelect_t motor, uint32_t speed)
{
    // 保存当前状态
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    StepperState_t previous_state = *motor_state;
    uint32_t previous_speed = *motor_speed;
    
    // 设置反转状态
    Stepper_SetState(motor, STEPPER_BACKWARD, speed);
    
    // 执行2048步完成360度旋转
    for(int i = 0; i < 2048; i++) {
        Stepper_Process(motor);
    }
    
    // 停止电机
    Stepper_Stop(motor);
    
    // 恢复之前的状态
    *motor_state = previous_state;
    *motor_speed = previous_speed;
}

// 步进电机正转指定角度函数
void Stepper_ForwardAngle(MotorSelect_t motor, uint32_t angle, uint32_t speed)
{
    // 保存当前状态
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    StepperState_t previous_state = *motor_state;
    uint32_t previous_speed = *motor_speed;
    
    // 计算需要转动的步数
    uint32_t steps = (angle * 2048) / 360;
    
    // 设置正转状态
    Stepper_SetState(motor, STEPPER_FORWARD, speed);
    
    // 执行指定步数
    for(uint32_t i = 0; i < steps; i++) {
        Stepper_Process(motor);
    }
    
    // 停止电机
    Stepper_Stop(motor);
    
    // 恢复之前的状态
    *motor_state = previous_state;
    *motor_speed = previous_speed;
}

// 步进电机反转指定角度函数
void Stepper_BackwardAngle(MotorSelect_t motor, uint32_t angle, uint32_t speed)
{
    // 保存当前状态
    StepperState_t* motor_state = GetMotorState(motor);
    uint32_t* motor_speed = GetMotorSpeed(motor);
    StepperState_t previous_state = *motor_state;
    uint32_t previous_speed = *motor_speed;
    
    // 计算需要转动的步数
    uint32_t steps = (angle * 2048) / 360;
    
    // 设置反转状态
    Stepper_SetState(motor, STEPPER_BACKWARD, speed);
    
    // 执行指定步数
    for(uint32_t i = 0; i < steps; i++) {
        Stepper_Process(motor);
    }
    
    // 停止电机
    Stepper_Stop(motor);
    
    // 恢复之前的状态
    *motor_state = previous_state;
    *motor_speed = previous_speed;
}
